/// 验证函数 calProjectionMatrix_UVfrW 的正确性
///
#include <Eigen/Core>

#include "opencv2/imgcodecs.hpp"
#include <opencv2/imgproc.hpp>
#include "opencv2/highgui.hpp"

#include <vtkDICOMImageReader.h>
#include <vtkImageData.h>

#include <memath/MeVtkMath.h>
#include <memath/meTransform.h>
#include "CeresSolProjMatrix.h"

#include "algorithm/dicomdetect/Parse2DDicomImage.h"

using namespace std;

struct ProjData {
    vector<Eigen::Vector3d> ptsRuler2DIronBallTop_W;
    vector<Eigen::Vector3d> ptsRuler2DIronBallBot_W;
    vector<cv::Point2f> topVector_UV, botVector_UV;


    void printSelf() {
        vector<Eigen::Vector3d>::iterator it;
        cout << "ptsRuler2DIronBallTop_W = \n";
        for (it=ptsRuler2DIronBallTop_W.begin(); it != ptsRuler2DIronBallTop_W.end(); ++it) {
            cout << "    " << (*it)[0] << ", " << (*it)[1] << ", " << (*it)[2] << endl;
        }
        cout << "ptsRuler2DIronBallBot_W = \n";
        for (it=ptsRuler2DIronBallBot_W.begin(); it != ptsRuler2DIronBallBot_W.end(); ++it) {
            cout << "    " << (*it)[0] << ", " << (*it)[1] << ", " << (*it)[2] << endl;
        }
        cout << "topVector_UV = \n";
        foreach(const cv::Point2f &p, topVector_UV) {
            cout << "    " << p.x << ", " << p.y << endl;
        }
        cout << "botVector_UV = \n";
        foreach(const cv::Point2f &p, botVector_UV) {
            cout << "    " << p.x << ", " << p.y << endl;
        }
    }
};

static ProjData sProjData1 = {
    { // ptsRuler2DIronBallTop_W
        {118.5, 2.99994,     46.23},
        {81.7299, 2.99994,      83},
        {118.5, 2.99994,  119.77},
        {155.27, 2.99994,      83},
        {118.5, 2.99994,   56.23},
    },
    { // ptsRuler2DIronBallBot_W
      {    152, -73.0001,      113},
      {84.9999, -73.0001,      120},
      {84.9999, -73.0001,       46},
      {    152, -73.0001,       53},
      { 156.95, -73.0001,   117.95},
      { 140.06, -73.0001,   114.25},
    },
    {
        {972.274,1339.2},
        {637.24,1004.17},
        {972.274,669.131},
        {1307.31,1004.17},
        {972.274,1248.08},
    },
    {
        {1300.83,702.323},
        {632.498,632.498},
        {632.498,1370.65},
        {1300.83,1300.83},
        {1350.2,652.946 },
        {1181.72,689.854},
    }

//    8000 1031.5  6.31612e-14  -97436.9
//    0    1031.5  -8000        1.54256e+06
//    -0   1       6.12323e-17   875
//    0    0       0             1
};


int main(int argc, char *argv[])
{
    ProjData &projData = sProjData1;
    projData.printSelf();

    { // 第1种计算方法
        Matrix4d matrixP;
        CeresSolProjMatrix::calProjectionMatrix_UVfrW(projData.ptsRuler2DIronBallTop_W, projData.ptsRuler2DIronBallBot_W,
                                                      projData.topVector_UV, projData.botVector_UV, matrixP);
        cout << "matrixP = \n" << matrixP << endl;

        cout << "verify P:" << endl;
        foreach(const Eigen::Vector3d &p, projData.ptsRuler2DIronBallBot_W) {
            Eigen::Vector3d inP = p;
            Eigen::Vector3d outP;
            matrixP.multiplyPoint3D(inP.data(), outP.data());
            cout << "  outP:" << outP[0]/outP[2] << ", " << outP[1]/outP[2] << endl;
        }
    }

    { // 第2种计算方法
        cout << "使用ceres运行" << endl;
        vector<Eigen::Vector3d> pts_W;
        vector<cv::Point2f> p2f_uv;

        pts_W.insert(pts_W.end(), projData.ptsRuler2DIronBallTop_W.begin(), projData.ptsRuler2DIronBallTop_W.end());
        pts_W.insert(pts_W.end(), projData.ptsRuler2DIronBallBot_W.begin(), projData.ptsRuler2DIronBallBot_W.end());

        p2f_uv.insert(p2f_uv.end(), projData.topVector_UV.begin(), projData.topVector_UV.end());
        p2f_uv.insert(p2f_uv.end(), projData.botVector_UV.begin(), projData.botVector_UV.end());
        Matrix4d matrixP;
        CeresSolProjMatrix::calProjectionMatrix_UVfrW(pts_W, p2f_uv, matrixP);
        cout << "matrixP = \n" << matrixP << endl;
    }

    return 0;
}
